#include "modbus.h"
#include "usart.h"
#include "string.h"

int modbusTimeoutCount=0;
int modbusRxFlag=0;

uint8_t modbusRxBuf[MODBUS_RX_BUF_SIZE];
int modbusRxLen=0;
uint8_t modbusTxBuf[MODBUS_TX_BUF_SIZE];
int modbusTxLen=0;
uint16_t modbusRegister[MODBUS_REG_SIZE]={0};

uint16_t crcRecv;
uint16_t crcCalc;
	
int frameCount=0;

int modbus_06_flag = 0;
uint16_t modbus_06_addr = 0;

const uint16_t polynom = 0xA001;

static void modbus_delay(void)
{
	int i;
	int j;
	
	for(i=0;i<48;i++)
	{
		for(j=0;j<1000;j++);
	}
}
	
uint16_t crc16(uint8_t *ptr, uint16_t len)
{
    uint8_t i;
    uint16_t crc = 0xffff;

	if(len>1024)
	{
		return 0;
	}
	
    if (len == 0) 
	{
        len = 1;
    }
    while (len--) 
	{
        crc ^= *ptr;
        for (i = 0; i<8; i++)
        {
            if (crc & 1) 
            {
                crc >>= 1;
                crc ^= polynom;
            }
            else 
            {
                crc >>= 1;
            }
        }
        ptr++;
    }
    
    return(crc);
}

void modbus_send_buf(uint8_t * buf, int len)
{
	HAL_HalfDuplex_EnableTransmitter(&huart1);
	
	uart_send_buf(buf,len);
	
	HAL_Delay(2); //if remove, will communication fail
	
	HAL_HalfDuplex_EnableReceiver(&huart1);
}

void modbus_03(uint8_t * buf, int len)
{
	uint16_t addr;
	int dataLen;
	uint16_t crc;
	int i;
		
	addr = buf[2]<<8 | buf[3];
	dataLen = buf[4]<<8 | buf[5];
	
	modbusTxBuf[0] = 1;
	modbusTxBuf[1] = 0x03;
	modbusTxBuf[2] = dataLen*2;
	
	for(i=0;i<dataLen;i++)
	{
		modbusTxBuf[3 + i * 2] = modbusRegister[addr+i]>>8;
		modbusTxBuf[3 + i * 2 + 1] = modbusRegister[addr+i];
	}
	
	modbusTxLen = dataLen*2 + 5;
	
	crc = crc16(modbusTxBuf,modbusTxLen-2);
	modbusTxBuf[modbusTxLen-2] = crc;
	modbusTxBuf[modbusTxLen-1] = crc>>8;
	
	modbus_send_buf(modbusTxBuf,modbusTxLen);
}
	
void modbus_06(uint8_t * buf, int len)
{
	uint16_t addr;
	uint16_t data;
	
	addr = buf[2]<<8 | buf[3];
	data = buf[4]<<8 | buf[5];
	
	modbusRegister[addr] = data;
	modbus_06_flag = 1;
    modbus_06_addr = addr;
    
	memcpy(modbusTxBuf,buf,8);
	modbusTxLen = 8;
	
	modbus_send_buf(modbusTxBuf,modbusTxLen);        
}
	
void modbus_rx_buf_remove_head(void)
{
	int i;
	
	if(modbusRxLen>0)
	{
		for(i=0;i<modbusRxLen;i++)
		{
			modbusRxBuf[i] = modbusRxBuf[i+1];
		}
		
		modbusRxLen--;
	}
}

void modbus_frame_get(void)
{
	if(modbusRxLen>0)
	{
		if(modbusRxBuf[0]!=1)
		{
			modbus_rx_buf_remove_head();
			
			return;
		}
	}
	
	if(modbusRxLen>=8)
	{
		modbusRxFlag = 1;
	}
}

void modbus_poll(void)
{	
	int i=0;
	uint8_t cmd;
	(void)i;
	
	if(modbusRxFlag)
	{
		crcRecv = modbusRxBuf[modbusRxLen-2] | modbusRxBuf[modbusRxLen-1]<<8;
		crcCalc = crc16(modbusRxBuf,modbusRxLen-2);
		if(crcCalc == crcRecv)
		{
			frameCount++;
			
			modbusTxBuf[0] = 1;
			cmd = modbusRxBuf[1];
			if(modbusRxBuf[0]==1)
			{
				switch(cmd)
				{
					case 0x03:
						modbus_03(modbusRxBuf,modbusRxLen);
						break;
					case 0x06:
						modbus_06(modbusRxBuf,modbusRxLen);
						break;
					default:
						break;
				}
			}
		}
		
		memset(modbusRxBuf,0,MODBUS_RX_BUF_SIZE);
		modbusRxLen = 0;
		modbusRxFlag = 0;
		modbusRxLen = 0;
	}
}

void modbus_rx_callback(uint8_t data)
{
	modbusRxBuf[modbusRxLen] = data;
	modbusRxLen++;
	if(modbusRxLen >= MODBUS_RX_BUF_SIZE)
	{
		modbusRxLen = 0;
	}
		
	modbusTimeoutCount = 0;
}

void modbus_timer_callback(void)
{
	if(modbusRxLen>0)
	{
		modbusTimeoutCount++;
		if(modbusTimeoutCount>10)
		{
			modbusRxFlag = 1;			
		}
	}
}



